import cv2 as cv
import numpy as np
import serial

lower_pink = np.array([175, 150, 100])
upper_pink = np.array([255, 255, 255])

img = cv.imread('./test/test.jpg')
img = cv.GaussianBlur(img, (3, 3), 0)
img_hsv = cv.cvtColor(img, cv.COLOR_BGR2HSV_FULL)

mask = cv.inRange(img_hsv, lower_pink, upper_pink)
cv.imshow('test', mask)
cv.moveWindow('test', 100, 100)

kernel = np.ones((5, 5), np.uint8)

mask = cv.erode(mask, kernel, iterations=2)
cv.imshow('mask', mask)

mask = cv.morphologyEx(mask, cv.MORPH_OPEN, kernel, iterations=1)
cv.imshow('morpho', mask)

mask = cv.dilate(mask, kernel, iterations=3)
cv.imshow('dilate', mask)

res = cv.bitwise_and(img, img, mask=mask)
cv.imshow('bitwise_and', res)

contours, heir = cv.findContours(mask, cv.RETR_EXTERNAL, cv.CHAIN_APPROX_NONE)

print(contours)
print(heir)

for contour in contours:
    point = contour.mean(axis=0)
    print(point[0])
    center = (int(point[0, 0]), int(point[0, 1]))
    cv.line(img, center, center, (255, 0, 0), 10)

    point = np.c_[point, np.array(1)].T
    print(point)
    M = np.arange(9).reshape(3, 3)
    M = np.matrix(M)
    print(M)
    a = M.dot(point)
    print(a)
    point = np.array([a[0, 0] / a[2, 0], a[1, 0] / a[2, 0]])
    print(point)

cv.imshow('result', img)


cv.waitKey(0)
cv.destroyAllWindows()


